//////////////////////////////
// trame
void send_start_trame(void)
{
  Serial.print("#");
}
void send_end_trame(void)
{
  Serial.println("$");
}

//////////////////////////////
// error
void send_error(String err)
{
  char e[256];
  err.toCharArray(e, sizeof(e));
  send_error(e);
}
void send_error(char *error)
{
  send_start_trame();
  Serial.write("error:");
  Serial.write(error);
  send_end_trame();
}

//////////////////////////////
// message
void send_message(String msg)
{
  char m[256];
  msg.toCharArray(m, sizeof(m));
  send_message(m);
}
void send_message(char *msg)
{
  send_start_trame();
  Serial.write("message:");
  Serial.write(msg);
  send_end_trame();
}

//////////////////////////////
// version
void send_version(void)
{
  send_start_trame();
  Serial.write("version:name=roby_bt,version=0.9.1");
  send_end_trame();
}

//////////////////////////////
// setting
void send_setting(void)
{
  send_start_trame();
  Serial.print("setting:");
  Serial.print("send_status=");
  Serial.print(_send_status?"1":"0");
  send_end_trame();
}

void send_setting_body(void)
{
  send_start_trame();
  Serial.print("setting_body:");
  Serial.print("min_speed=0");
  Serial.print(",nice_speed=50");
  Serial.print(",max_speed=100");
  Serial.print(",min_dist=");
  Serial.print(body_ir_min);
  Serial.print(",max_dist=");
  Serial.print(body_ir_max);
  send_end_trame();
}

void send_setting_turret(void)
{
  send_start_trame();  
  Serial.print("setting_turret:");
  Serial.print("min_low=");
  Serial.print(turret_low_min);
  Serial.print(",mid_low=");
  Serial.print(turret_low_mid);
  Serial.print(",max_low=");
  Serial.print(turret_low_max);
  Serial.print(",min_high=");
  Serial.print(turret_high_min);
  Serial.print(",mid_high=");
  Serial.print(turret_high_mid);
  Serial.print(",max_high=");
  Serial.print(turret_high_max);
  Serial.print(",min_dist=");
  Serial.print(turret_ir_min);
  Serial.print(",mid_dist=");
  Serial.print(turret_ir_max);
  Serial.print(",max_dist=");
  Serial.print(turret_us_max);
  send_end_trame();
}
void send_setting_ray(void)
{
  send_start_trame();
  Serial.print("setting_ray:");
  Serial.print("count=");
  Serial.print(_beam_count);
  Serial.print(",delay=");
  Serial.print(_beam_delay);
  Serial.print(",sensors=");
  Serial.print(_beam_sensors);
  Serial.print(",precision=");
  Serial.print(_beam_precision);
  send_end_trame();
}

//////////////////////////////
// status
void send_status_hitsensors(void)
{
  boolean bl = get_hit_switch(switch_body_back_left);
  boolean bc = get_hit_switch(switch_body_back_center);  
  boolean br = get_hit_switch(switch_body_back_right);

  send_start_trame();
  Serial.print("hit_sensors:");
  Serial.print("bl="); 
  Serial.print(bl?"0":"1"); 
  Serial.print(",");
  Serial.print("bc="); 
  Serial.print(bc?"0":"1"); 
  Serial.print(",");
  Serial.print("br="); 
  Serial.print(br?"0":"1"); 
  send_end_trame();
}
void send_status_turret(void)
{
  short dist = get_range_turret();

  send_start_trame();
  Serial.print("turret:");
  Serial.print("low=");
  Serial.print(current_turret_position_low); 
  Serial.print(",");
  Serial.print("high="); 
  Serial.print(current_turret_position_high); 
  Serial.print(",");
  Serial.print("dist="); 
  Serial.print(dist);
  send_end_trame();
}
void send_status_body(void)
{
  short dist = get_range_body();

  send_start_trame();
  Serial.print("body:");
  Serial.print("bear=");
  Serial.print(body_compass_bearing);
  Serial.print(",");
  Serial.print("pitch=");
  Serial.print(body_compass_pitch);
  Serial.print(",");
  Serial.print("roll=");
  Serial.print(body_compass_roll);
  Serial.print(",");
  Serial.print("dist=");
  Serial.print(dist);
  send_end_trame();
}
void send_body_moving_distance(void)
{
  send_start_trame();
  Serial.print("latest_distance:");
  Serial.print("left=");
  Serial.print(body_lastmove_distance_left);
  Serial.print(",");
  Serial.print("right=");
  Serial.print(body_lastmove_distance_right);
  send_end_trame();
}

void send_target_ok(int id)
{
  send_start_trame();
  Serial.print("target:");
  Serial.print("id=");
  Serial.print(id);
  Serial.print(",");
  Serial.print("result=ok");
  send_end_trame();
}

